#include "WPILib.h"
#include "Joysticks.hpp"
#include "RobotDrive2012.hpp"

/**
 * This is a demo program showing the use of the RobotBase class.
 * The SimpleRobot class is the base of a robot application that will automatically call your
 * Autonomous and OperatorControl methods at the right time as controlled by the switches on
 * the driver station or the field controls.
 */ 
class RobotDemo : public SimpleRobot
{
	//RobotDrive myRobot; // robot drive system
	XBox360 stick; // only joystick
	RobotDrive myRobot2011;

public:
	RobotDemo(void):
		//myRobot(1, 2),	// these must be initialized in the same order
		stick(1),		// as they are declared above.
		myRobot2011(4,2)
	{
		//myRobot.SetExpiration(0.1);
		myRobot2011.SetExpiration(0.1);
	}

	/**
	 * Drive left & right motors for 2 seconds then stop
	 */
	void Autonomous(void)
	{
		//myRobot.SetSafetyEnabled(false);
		//myRobot.Drive(0.5, 0.0); 	// drive forwards half speed
		//Wait(2.0); 				//    for 2 seconds
		//myRobot.Drive(0.0, 0.0); 	// stop robot
	}

	/**
	 * Runs the motors with arcade steering. 
	 */
	void OperatorControl(void)
	{
		//Deadband myDeadband;
		float XDrive, YDrive;
		//myRobot.SetSafetyEnabled(true);
		myRobot2011.SetSafetyEnabled(true);
		while (IsOperatorControl())
		{
			XDrive=stick.GetRightStickX();YDrive=stick.GetRightStickY();
			if(stick.GetButtonA()||stick.GetButtonX()) {XDrive=0;YDrive=0;}
			printf("X=%f, Y=%f\n", XDrive,YDrive);
			myRobot2011.ArcadeDrive(-YDrive, XDrive);
			//myRobot.ArcadeDrive(stick); // drive with arcade style (use right stick)
			Wait(0.005);				// wait for a motor update time
		}

	}
};

START_ROBOT_CLASS(RobotDemo);

